from common import convert_to_four_digit_string
import serial
import serial.tools.list_ports
import time
from simulator import Simulator
class UART:
    def __init__(self,port = None,baudrate = None,timeout = 0.1):
        #串口部分
        self.port = port
        self.baudrate = baudrate
        self.timeout = timeout
        self.ser = serial.Serial()
        self.baudrate_options=["9600","19200","115200"]
        self.pwm=[0,0,0,0,0]
        self.old_pwm=[0,0,0,0,0]
        self.count=int(0)
        #仿真部分
        self.sim=Simulator()
        
        
        
    def open_port(self):
        self.ser.port=self.port
        self.ser.baudrate=self.baudrate
        self.ser.timeout=self.timeout
        
        try:
            self.ser.open()
        except Exception as e:
            print(f"Error opening the serial port: {e}")
        if self.ser.isOpen():
            print("串口已成功打开")
            self.servo_reset()
            return True
        else:
            print("串口未成功打开")
            return False
        
    def close_port(self):
        self.ser.close()
    def send_data(self,data):
        self.ser.write(data)
    def read_data(self):
        return self.ser.readline()
    def read_data_until(self,data):
        return self.ser.read_until(data)
    def read_data_until_timeout(self,data,timeout):
        return self.ser.read_until(data,timeout)
        
    def send_command(self,joint_angles,time):
        if self.ser.isOpen():
            self.count+=1
            self.pwm[0]=(int)(1500+1400*joint_angles[0]/180)
            self.pwm[1]=(int)( 800+1400*joint_angles[1]/180)
            self.pwm[2]=(int)(1500-1400*joint_angles[2]/180)
            self.pwm[3]=(int)(1500-1000*joint_angles[3]/135)
            self.pwm[4]=(int)(1500+1400*joint_angles[4]/180)
            command=[]
            for i in range(5):
                if abs(self.pwm[i]-self.old_pwm[i])>=2:#pwm差值太小就别法信息了，防止引起抖动
                    command.append('#00'+str(i)+'P'+convert_to_four_digit_string(self.pwm[i])+'T'+time+'!')
                    self.old_pwm[i]=self.pwm[i]
            command_string='{'
            for i in range(len(command)):
                command_string+=command[i]
            command_string+='}'
            self.ser.write(command_string.encode('utf-8'))
        if self.sim.is_simulating==True:
            self.sim.step_sim(joint_angles)
        
        
    
    def set_grapper_pwm(self,pwm,_time='0500'):
        time.sleep(0.01)
        if self.ser.isOpen():
            #串口部分
            command_string='#005P'+convert_to_four_digit_string(int(pwm))+'T'+_time+'!'
            self.ser.write(command_string.encode('utf-8'))
        #仿真部分
        if self.sim.is_simulating==True:
            self.sim.set_grapper(pwm)
    
    def servo_reset(self):
        
        self.send_command([0,90,0,0,0],time='1000')
        time.sleep(1)
        self.set_grapper_pwm(1300)
        time.sleep(0.01)
        
    def get_serial_port(self):
        port_list = []
        portlist  = list(serial.tools.list_ports.comports())
        for port in portlist:
            port_list.append(port.device)
        return port_list 
# uart=UART('COM3',115200)
# uart.open_port()

# import time
# time.sleep(4)
# uart.send_command([0, 96,-36, -96, 0],time='1000')
# time.sleep(2)
# uart.set_grapper_pwm(1900,'1000')
# uart.send_command([0.0, 138.62230082742636, 15.828464537153678, -87.45076536458004, 180],time='2000')
# command_string='#005P1200T0200!'
# print(command_string)
# uart.ser.write(command_string.encode('utf-8'))
# uart.ser.write(command_string.encode('utf-8'))
# uart.close_port()
# uart.send_command([0,90,60,30,0],time='1000')
# uart.send_command([90,90,30,30,0],time='0050')
# time.sleep(0.01)
# uart.send_command([0,90,30,60,0],time='0100')

# uart.send_command([0,90,60,30,0],time='1000')
# uart.send_command([0,90,30,30,0],time='1000')
# uart.send_command([0,90,30,60,0],time='1000')

# uart.send_command([0,90,60,30,0],time='1000')
# uart.send_command([0,90,30,30,0],time='1000')
# uart.send_command([0,90,30,60,0],time='1000')

# uart.send_command([0,90,60,30,0],time='1000')
# uart.send_command([0,90,30,30,0],time='1000')
# uart.send_command([0,90,30,60,0],time='1000')

# uart.send_command([0,90,60,30,0],time='1000')
# uart.send_command([0,90,30,30,0],time='1000')
# uart.send_command([0,90,30,60,0],time='1000')

# uart.send_command([0,90,60,30,0],time='1000')
# uart.send_command([0,90,30,30,0],time='1000')
# uart.send_command([0,90,30,60,0],time='1000')

# uart.send_command([0,90,60,30,0],time='1000')
# uart.send_command([0,90,30,30,0],time='1000')
# uart.send_command([0,90,30,60,0],time='1000')



